/*
 * @Description: 
 * @Autor: kaikaima
 * @Date: 2021-04-06 19:37:11
 * @LastEditors: kaikaima
 * @LastEditTime: 2021-04-17 12:03:28
 */
#include <iostream>
#include "Matrix.h"

#ifndef __hhxxttxs_Linearequations__
#define __hhxxttxs_Linearequations__

class L_Equations : public Matrix{
    private:
    double *B;
    public:
    L_Equations(int _a) : Matrix(_a){
        B=new double [_a];
    };
    ~L_Equations(){
        delete B;
    };
    void set(double **_a){
        Matrix::set(_a);
        for(int i=0;i<N;i++)
            B[i]=_a[N][i];
    };
    void set(double* _a){
        for(int i=0;i<N;i++)
            B[i]=_a[i];
    };
//////////////////////////////////////////
///
///     FOR GAUSS
///
//////////////////////////////////////////
    private:
    void G_backward_sweep();
    void G_forward_sweep();
    int Gauss();
    int Gauss_original();
    int max(int _i) const;
    public:
    double* Gauss_solve();
    double* Gauss_solve_original();
//////////////////////////////////////////
///
///     FOR Cholesky
///
//////////////////////////////////////////
    private:
    void C_backward_sweep();
    void C_forward_sweep();    
    int Cholesky();
    int Cholesky_modified();
    public:    
    double* Cholesky_solve();
    double* Cholesky_solve_modified();
};

////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
///////////////////////////////////////////////////////

/**
 * @description: solve upper triangular equations Ux=y
 * @param {*}
 * @return {*}
 * @time_complexity: n^2
 * @author: kaikaima
 */
void L_Equations::G_backward_sweep() {
    for(int i=N-1;i>0;i--){
        B[i]/=A[i][i];
        for(int j=0;j<i;j++)
            B[j]-=(B[i]*A[j][i]);
    }
    B[0]/=A[0][0];
};

/**
 * @description: solve lower triangular equations Ly=Pb
 * @param {*}
 * @return {*}
 * @time_complexity: n^2
 * @author: kaikaima
 */
void L_Equations::G_forward_sweep() {
    for(int i=0;i<N-1;i++)
        for(int j=i+1;j<N;j++)
            B[j]-=(B[i]*A[j][i]);
};

/**
 * @description: Gaussion elimination with partial pivoting method, change Ax=b to LUx=Pb
 * @param {*}
 * @return {*}
 * @time_complexity: 2/3*n^3
 * @author: kaikaima
 */
int L_Equations::Gauss() {
    for(int i=0;i<N-1;i++){
        int p=max(i);
        {double *tmp=A[p]; A[p]=A[i]; A[i]=tmp;}
        {double tmp=B[p]; B[p]=B[i]; B[i]=tmp;}
        if(A[i][i]!=0){
            for(int j=i+1;j<N;j++)
                A[j][i]/=A[i][i];
            for(int j=i+1;j<N;j++)
                for(int k=i+1;k<N;k++)
                    A[j][k]-=(A[j][i]*A[i][k]);
        }
        else{
            std::cout<<"Error: Singular Matrix"<<std::endl;
            return -1;
        }
    }
    return 0;
};

/**
 * @description: Normal Gaussion elimination
 * @param {*}
 * @return {*}
 * @time_complexity: 2/3*n^3
 * @author: kaikaima
 */
int L_Equations::Gauss_original() {
    for(int i=0;i<N-1;i++)
        if(A[i][i]!=0){
            for(int j=i+1;j<N;j++)
                A[j][i]/=A[i][i];
            for(int j=i+1;j<N;j++)
                for(int k=i+1;k<N;k++)
                    A[j][k]-=(A[j][i]*A[i][k]);
        }
        else{
            std::cout<<"Error: Singular Matrix"<<std::endl;
            return -1;
        }
    return 0;
}

/**
 * @description: find the max element in { A(k,_i) | k=_i:N }
 * @param {*} column coordinate _i
 * @return {*} p which let A(p,_i) = MAX{ A(k,_i) | k=_i:N }
 * @time_complexity: n
 * @author: kaikaima
 */
int L_Equations::max(int _i) const {
    int result=_i;
    for(int i=_i;i<N;i++)
        if(fabs(A[result][_i])<fabs(A[i][_i]))
            result=i;
    return result;
};

/**
 * @description: 
 * @param {*}
 * @return {*}
 * @author: kaikaima
 */
double* L_Equations::Gauss_solve() {
    if(!Gauss()){
        G_forward_sweep();
        G_backward_sweep();
    }
    return B;
};

double* L_Equations::Gauss_solve_original() {
    if(!Gauss_original()){
        G_forward_sweep();
        G_backward_sweep();
    }
    return B;
};

////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
///////////////////////////////////////////////////////

/**
 * @description: solve upper triangular equations Ux=y while U is stored as L^T
 * @param {*}
 * @return {*}
 * @time_complexity: n^2
 * @author: kaikaima
 */
void L_Equations::C_backward_sweep() {
    for(int i=N-1;i>0;i--){
        B[i]/=A[i][i];
        for(int j=0;j<i;j++)
            B[j]-=(B[i]*A[i][j]);
    }
    B[0]/=A[0][0];
};

void L_Equations::C_forward_sweep() {
    for(int i=0;i<N-1;i++){
        B[i]/=A[i][i];
        for(int j=i+1;j<N;j++)
            B[j]-=(B[i]*A[j][i]);
    }
    B[N-1]/=A[N-1][N-1];
};

/**
 * @description: Cholesky factorization : Square root method
 * @param {*}
 * @return {*}
 * @time_complexity: 1/3*n^3
 * @author: kaikaima
 */
int L_Equations::Cholesky() {
    for(int i=0;i<N;i++){
        A[i][i]=sqrt(A[i][i]);
        for(int j=i+1;j<N;j++)
            A[j][i]/=A[i][i];
        for(int j=i+1;j<N;j++)
            for(int k=j;k<N;k++)
                A[k][j]-=(A[k][i]*A[j][i]);
    }
    return 0;
};

int L_Equations::Cholesky_modified() {
    double v[N]={0};
    for(int i=0;i<N;i++){
        for(int j=0;j<i;j++)
            v[j]=A[i][j]*A[j][j];
        for(int j=0;j<N;j++)
            if(j<i)
            A[i][i]-=(A[i][j]*v[j]);
            else if(j>i){
                for(int k=0;k<i;k++)
                    A[j][i]-=(A[j][k]*v[k]);
                A[j][i]/=A[i][i];
            }
            else continue;
    }
    return 0;
};

double* L_Equations::Cholesky_solve() {
    if(!Cholesky()){
        C_forward_sweep();
        C_backward_sweep();
    }
    return B;
};

double* L_Equations::Cholesky_solve_modified() {
    if(!Cholesky_modified()){
        G_forward_sweep();//注意这里是单位下三角矩阵故用G
        for(int i=0;i<N;i++)
            B[i]/=A[i][i];
        /**
         * @description: 同样，接下来对单位上三角阵操作，相当于G_backward_sweep_T
         * @param {*}
         * @return {*}
         * @time_complexity: 
         * @author: kaikaima
         */
        {
            for(int i=N-1;i>0;i--)
                for(int j=0;j<i;j++)
                    B[j]-=(B[i]*A[i][j]);
        };
    }
    return B;
};

#else
//DO NOTHING!
#endif